package XYC;

import java.awt.geom.Point2D;

/**
 * Includes all the utility methods that used by YichiRobot.
 * Code original written by Aaron Herres.
 * @author Yichi Xu.
 * */
public class RoboCodeUtil {
/**
* Return in degree that robot should head to in order to reach the target.
* @param p1 a Point2D object that indicates the starting position.
* @param p2 a Point2D object that indicates the target position
* @param curHeading a double instance that indicates the current heading
* @return a double instance that indicates the supposed heading
* */
  public static double computeHeading(Point2D p1, Point2D p2, double curHeading) {
  double rawHeading = Math.atan2((p1.getX() - p2.getX()),(p1.getY() - p2.getY()));
  rawHeading = (rawHeading * 180 / Math.PI) - curHeading;
  return normalizeHeading(rawHeading);
  }
/**
* Convert any angle to a angle within -180 to 180. Return the angle in degree.
* @param rawHeading a double instance that indicates the original angle
* @return a double instance that indicates the new angle
* */
  public static double normalizeHeading(double rawHeading) {
  double newHeading = rawHeading;
  newHeading %= 360;
  if (newHeading < -180) {
  newHeading += 360;
  }
  if (newHeading > 180) {
  newHeading -= 360;
  }
  return newHeading;
  }
}